#include"../../include/ros_detection_tracking/project.h"

using namespace Eigen;
using namespace std;



namespace DetectandTract{

    void projector::initParams()
    {
        std::string pkg_loc = ros::package::getPath("ros_nus_tracking");
        std::ifstream infile(pkg_loc + "/cfg/initial_params.txt");
        infile >> i_params.boxs_topic;
        infile >> i_params.lidar_topic;
        //std::cout<<i_params.camera_topic<<std::endl;
        double_t camtocam[16];
        double_t cameraIn[12];
        double_t RT[16];

        // 读取cam_cam矩阵参数
        for (int i = 0; i < 16; i++){
            infile >> camtocam[i];
        }
        cv::Mat(4, 4, 6, &camtocam).copyTo(i_params.camtocam_mat);//cameratocamera params
        // std::cout<<i_params.camtocam_mat<<std::endl;
        // i_params.cam_cam(4,4);
        i_params.cam_cam = mat_to_eigen(i_params.camtocam_mat);
        std::cout<<i_params.cam_cam<<std::endl;

        // 读取相机内参
        for (int i = 0; i < 12; i++){
            infile >> cameraIn[i];
        }
        cv::Mat(3, 4, 6, &cameraIn).copyTo(i_params.cameraIn);//cameraIn params
        // std::cout<<i_params.cameraIn<<std::endl;
        // 转化为eigen矩阵 
        // i_params.cam_in(3,4);
        i_params.cam_in = mat_to_eigen(i_params.cameraIn);
        std::cout<<i_params.cam_in<<std::endl;

        // 读取相机外参
        for (int i = 0; i < 16; i++){
            infile >> RT[i];
        }
        cv::Mat(4, 4, 6, &RT).copyTo(i_params.RT);//lidar to camera params
        // std::cout<<i_params.RT<<std::endl;
        // i_params.cam_RT(4,4);
        i_params.cam_RT = mat_to_eigen(i_params.RT);
        std::cout<<i_params.cam_RT<<std::endl;
    }


    void projector::projection_callback(const yolov5_ros_msgs::BoundingBoxes::ConstPtr &boxs_Ptr, 
                                    const sensor_msgs::PointCloud2::ConstPtr &pc)
        {
            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
            pcl::fromROSMsg(*pc, *cloud);
            // 读取识别框 指针转对象 总消息
            yolov5_ros_msgs::BoundingBoxes boxs = *boxs_Ptr;
            // 为遍历作准备 读取总消息中的识别框消息
            auto box_list = boxs.bounding_boxes;
            // 为框选点云设置容器
            unordered_map<int,pcl::PointCloud<pcl::PointXYZ> > map_pcloud;
            // 结果乱装容器
            pcl::PointCloud<pcl::PointXYZ> rel_cloud;
            // 人框
            vector<yolov5_ros_msgs::BoundingBox> box_person_vel;
            for (auto it=box_list.begin();it != box_list.end(); it++)
            {
                if (it->Class == "2.0") // 0.0是人的类别代码
                {
                    box_person_vel.push_back(*it);
                    pcl::PointCloud<pcl::PointXYZ> box_pcloud;
                    map_pcloud.insert(make_pair(it->num, box_pcloud));
                }
                
            }
            
            std::cout<<"get pointcloud and boundboxes data"<<std::endl;
            
            // 原始点云 齐次4维 本质3维
            Vector4f X;
            // 投射平面坐标 齐次3维度 本质2维
            Vector3f Y;
            // cv::Mat X(4,1,cv::DataType<double>::type);
            // cv::Mat Y(3,1,cv::DataType<double>::type);
            // cout<<ros::Time::now()<<endl;
            

            // 开始投射，遍历开始
            for(auto it = cloud->points.begin(); it != cloud->points.end(); it++)
            {
                if(it->x > maxX || it->x < 0.0 || abs(it->y) > maxY || it->z < minZ ) 
                {
                    continue;
                }
                auto tst = *it;
                X(0,0) = it->x;
                X(1,0) = it->y;
                X(2,0) = it->z;
                X(3,0) = 1;

                Y = i_params.cam_in * i_params.cam_cam* i_params.cam_RT * X;  //tranform the point to the camera coordinate
                cout<< Y <<endl;
                cout<< "oo"<<endl;
                // cout<<Y(0,2)<<" 02ww"<<endl;
                // cv::Point pt;
                // pt.x = Y.at<double>(0,0) / Y.at<double>(0,2);
                // pt.y = Y.at<double>(1,0) / Y.at<double>(0,2);
                int temp_x = Y(0,0)/Y(2,0);
                int temp_y = Y(1,0)/Y(2,0);
                // cout<< temp_x << "is tempx"<<endl;
                for (auto const e:box_person_vel)
                {
                    int box_xmin = e.xmin * shrink_factor;
                    int box_xmax = e.xmax * shrink_factor;
                    int box_ymin = e.ymin * shrink_factor;
                    int box_ymax = e.ymax * shrink_factor;
                    // cout<<box_xmin<<" is box_xmin"<<endl;
                    // cout<<box_xmax<<" is box_xmax"<<endl;
                    int object_id = e.num;
                    if( (temp_x < box_xmax && temp_x > box_xmin) && (temp_y > box_ymin && temp_y < box_ymax) )
                    {
                        // cout<<"here is point  "<<endl;
                        auto it_point = map_pcloud.find(object_id);
                        it_point->second.push_back(*it);
                        // auto testt = map_pcloud[object_id];
                        // map_pcloud[object_id].push_back(*it);
                        // rel_cloud.push_back(*it);
                        break;
                    }
                }
               
            }

            // 测试代码
            // pcl::PointCloud<pcl::PointXYZ> testcloud; //point cloud msg  
            // sensor_msgs::PointCloud2 output; //PointCloud2 msg

            // testcloud.width=500;
            // testcloud.height=2;
            // testcloud.points.resize(testcloud.width*testcloud.height);

            // for(size_t i=0; i<testcloud.points.size(); ++i)
            // {
            //         testcloud.points[i].x=1024*rand()/(RAND_MAX+1.0f);
            //         testcloud.points[i].y=1024*rand()/(RAND_MAX+1.0f);
            //         testcloud.points[i].z=1024*rand()/(RAND_MAX+1.0f);
            // }

            // cout<<ros::Time::now()<<"is time"<<endl;


            // 分割分类点云结束
            // 计算中心点 准备发布people话题

            people_msgs::PositionMeasurementArray people_total;

            for (auto const e:map_pcloud)
            {
                // 创立person消息
                people_msgs::PositionMeasurement person;
                person.header = pc->header;
                person.header.stamp = ros::Time::now();
                Vector4f centor_point;
                pcl::compute3DCentroid(e.second,centor_point);
                person.pos.x = centor_point(0,0);
                person.pos.y = centor_point(1,0);
                person.pos.z = centor_point(2,0);

                person.object_id = to_string(e.first);
                float reliability = 0;
                for (auto const box:box_person_vel)
                {
                    if(box.num == e.first)
                    {
                        reliability = box.probability;
                    }
                }
                
                person.reliability = reliability;
                people_total.people.push_back(person);
            }
            people_total.header = pc->header;
            people_pub.publish(people_total);


            // 聚合各分类点云 准备可视化
            pcl::PointCloud<pcl::PointXYZ> rviz_cloud;
            for (auto const e:map_pcloud)
            {
                rviz_cloud += e.second;
            }

            // 显示点云 结果可视化
            sensor_msgs::PointCloud2 cloud_msg;
            pcl::toROSMsg(rviz_cloud, cloud_msg);
            cloud_msg.header = pc->header;
            rviz_point_pub.publish(cloud_msg);

            // Publish the image projection
            ros::Time time = ros::Time::now();
            // cout<<"x y "<<endl;
            map_pcloud.clear();
            std::cout<<"project picture is published!"<<std::endl;
        }

    
    void projector::matrix_to_transfrom(Eigen::MatrixXf &matrix, tf::Transform & trans)
    {
        
    }

    Eigen::MatrixXf projector::mat_to_eigen(cv::Mat &from_mat)
    {
        int row = from_mat.rows;
        int col = from_mat.cols;
        Eigen::MatrixXf m(row, col);
        cv::cv2eigen(from_mat,m);
        return m;
    }


    projector::projector() 
    {
        ros::NodeHandle nh("~");
        initParams();

        ros::Publisher project_img_pub;

        message_filters::Subscriber<yolov5_ros_msgs::BoundingBoxes> boxs_sub(nh, i_params.boxs_topic, 5);
        message_filters::Subscriber<sensor_msgs::PointCloud2> pcl_sub(nh, i_params.lidar_topic, 5);

        typedef message_filters::sync_policies::ApproximateTime<yolov5_ros_msgs::BoundingBoxes, sensor_msgs::PointCloud2> MySyncPolicy;
        message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(5), boxs_sub, pcl_sub);
        sync.registerCallback(boost::bind(&projector::projection_callback, this, _1, _2));

        image_transport::ImageTransport imageTransport(nh);
        image_publisher = imageTransport.advertise("/project_pc_image", 20);
        rviz_point_pub = nh.advertise<sensor_msgs::PointCloud2> ("/rviz_point",1);
        people_pub = nh.advertise<people_msgs::PositionMeasurementArray> ("/people_mean_Array",1);
        ros::spin();
    }



}

int main(int argc, char **argv){
    ros::init(argc, argv, "project_pc_to_image");
    DetectandTract::projector projector;
    return 0;
}